import heapq


class Node:
    def __init__(self, position, parent=None):
        self.position = position
        self.parent = parent
        self.g = 0  # Cost from start node to current node
        self.h = 0  # Heuristic cost from current node to goal node
        self.f = 0  # Total cost: f = g + h

    def __eq__(self, other):
        return self.position == other.position

    def __lt__(self, other):
        return self.f < other.f

    def __repr__(self):
        return f"Node(pos={self.position}, f={self.f})"


def heuristic(a, b):
    """Manhattan distance heuristic"""
    return abs(a[0] - b[0]) + abs(a[1] - b[1])


def a_star_search(grid, start, goal):
    """
    A* Search algorithm to find the shortest path in a grid.
    :param grid: 2D list representing the grid (0 = free, 1 = obstacle)
    :param start: Tuple (x, y) for the start position
    :param goal: Tuple (x, y) for the goal position
    :return: List of positions representing the path from start to goal
    """
    open_list = []
    closed_list = []
    start_node = Node(start)
    goal_node = Node(goal)
    heapq.heappush(open_list, start_node)

    while open_list:
        current_node = heapq.heappop(open_list)
        closed_list.append(current_node)

        if current_node == goal_node:
            path = []
            while current_node:
                path.append(current_node.position)
                current_node = current_node.parent
            return path[::-1]  # Reverse the path

        neighbors = [(0, 1), (1, 0), (0, -1), (-1, 0)]  # Right, Down, Left, Up
        for move in neighbors:
            neighbor_position = (current_node.position[0] + move[0], current_node.position[1] + move[1])

            # Check if neighbor is within grid bounds
            if (0 <= neighbor_position[0] < len(grid) and
                    0 <= neighbor_position[1] < len(grid[0]) and
                    grid[neighbor_position[0]][neighbor_position[1]] == 0):
                neighbor_node = Node(neighbor_position, current_node)

                if neighbor_node in closed_list:
                    continue

                # Calculate costs
                neighbor_node.g = current_node.g + 1
                neighbor_node.h = heuristic(neighbor_node.position, goal_node.position)
                neighbor_node.f = neighbor_node.g + neighbor_node.h

                # Check if neighbor is in open list and has lower f value
                if any(node == neighbor_node and node.f <= neighbor_node.f for node in open_list):
                    continue

                heapq.heappush(open_list, neighbor_node)

    return None  # No path found


# Example usage
if __name__ == "__main__":
    grid = [
        [0, 0, 0, 0, 1],
        [1, 1, 0, 1, 1],
        [0, 0, 0, 0, 0],
        [0, 1, 1, 1, 0],
        [0, 0, 0, 0, 0]
    ]
    start = (0, 0)
    goal = (4, 4)

    path = a_star_search(grid, start, goal)
    if path:
        print("Path found:", path)
    else:
        print("No path found")
